#include "MPU6500.h"

#define Buf_SIZE 10
#define DATA_LEN 7

int16_t  MPU6500_FIFO[DATA_LEN][Buf_SIZE];	//数据缓存数组
static uint8_t Wr_Index = 0;	 	//数组下标
uint8_t Gyro_Off_started = 0;		//陀螺仪偏移计算完成标志
int16_t lastAx,lastAy,lastAz,
		lastGx,lastGy,lastGz;
static int16_t Gx_offset=0,Gy_offset=0,Gz_offset=0;	//陀螺仪偏移



/**
 * @Description 将数据写进FIFO
 */
static void MPU6500_NewVal(int16_t* buf,int16_t val) {
  	buf[Wr_Index] = val;
}


/**
 * @Description 读取FIFO数据中的平均值
 */
static int16_t MPU6500_GetAvg(int16_t* buf)
{
    int i;
	int32_t	sum = 0;
	for(i=0;i<Buf_SIZE;i++)
		sum += buf[i];
	sum = sum / Buf_SIZE;
	return (int16_t)sum;
}



/**
 * @Description 写MPU6500寄存器
 */
static void MPU6500_WriteReg(u8 reg, u8 data)
{
	MPU6500_CSL();				//拉低片选线，表示一个spi起始信号
	SPI3_ReadWrite_Byte(reg);	//发送设备寄存器地址
	SPI3_ReadWrite_Byte(data);	//发送写入寄存器的数据
	MPU6500_CSH();				//结束传输
}


/**
 * @Description 读MPU6500多个连续的寄存器
 */
static void MPU6500_ReadRegs(u8 reg, u8 length, u8 *data){
	u8 count = 0;
	SPI3_ReadWrite_Byte(reg|0x80);		//发送准备读取的寄存器的地址，最高位为1表示读操作
	for(count=0;count<length;count++){
		data[count] = SPI3_ReadWrite_Byte(0xff);	//读取一个字节的数据
	}
}

/**
 * 设置低通滤波器的频率
 */
static void MPU_Set_LPF(u16 lpf)
{
	u8 data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	MPU6500_WriteReg(MPU6500_RA_CONFIG,data);

}

/**
 * 设置陀螺仪数据输出的频率
 */
static void MPU_Set_GyroRate(u16 rate)
{
	u8 data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	MPU6500_WriteReg(MPU6500_RA_SMPLRT_DIV,data);	
 	MPU_Set_LPF(rate/2);	
}

/**
 * 设置加速度计数据输出的频率
 */
static void MPU_Set_AcclRate(u16 rate)
{
	MPU6500_WriteReg(MPU6500_RA_FF_THR,rate);  				
	delay_ms(1);
}

/**
* 复位MPU的所有寄存器
 */
void static MPU6500_Reset(void)
{
	MPU6500_WriteReg(MPU6500_RA_PWR_MGMT_1,0x80); 			//复位寄存器
	delay_ms(110);
	MPU6500_WriteReg(MPU6500_RA_SIGNAL_PATH_RESET,0x07);	//复位信号通路
	delay_ms(110);					 
}

/**
 * 设置时钟源
 */
static void MPU6500_SetClockSource(uint8_t source)
{
	MPU6500_WriteReg(MPU6500_RA_PWR_MGMT_1,source); 			//使用内部20M晶振作为时钟
	delay_ms(1);
}

/**
 * 设置陀螺仪量程
 */
void static MPU6500_Gyro_SetFsr(uint8_t fsr)
{	
	MPU6500_WriteReg(MPU6500_RA_GYRO_CONFIG,fsr); 			
	delay_ms(1);

}

/**
 * 设置加速计量程
 */
static void  MPU6500_Acc_SetFsr(uint8_t fsr)
{	
	MPU6500_WriteReg(MPU6500_RA_ACCEL_CONFIG,fsr);			
	delay_ms(1);

}

/**
 * @Description MPU6500初始化
 * 加速度量程+-2g，陀螺仪量程+-2000°/s，采样ADC精度为16位
 * 数据更新速度为500hz
 */
void MPU6500_Init(void)
{ 

	MPU6500_Reset();									//复位模块
	MPU6500_SetClockSource(INTERNAL_CLOCK);				//设置模块时钟源
	MPU6500_Acc_SetFsr(ACCEL_FS_2G);					//设置加速度计量程
	MPU_Set_AcclRate(A_DLPF_CFG);						//设置加速度数据输出速率
	MPU6500_Gyro_SetFsr(ACCEL_FS_2000DPS);				//设置陀螺仪量程
	MPU_Set_GyroRate(500);								//设置陀螺仪数据输出速率
	MPU6500_WriteReg(MPU6500_RA_PWR_MGMT_2,0x00); 		//开启加速度计、陀螺仪
	delay_ms(1);
}


/**
 * @Description 读取MPU6500原始数据、并计算平均滤波后的数据
 * @param gyro、acc：长度为3的数组首地址
 */
void MPU6500_readGyro_Acc(int16_t *gyro,int16_t *acc)
{
	static u8 buf[14];
	static int16_t gx,gy,gz;
	static int16_t ax,ay,az;
	
	MPU6500_CSL();
	MPU6500_ReadRegs(MPU6500_RA_ACCEL_XOUT_H,14,buf);	//读取加速度、陀螺仪、温度数据到buf中
	MPU6500_CSH();
	//将读取到的数据放进FIFO
	//加速度数据
	MPU6500_NewVal(&MPU6500_FIFO[0][0],(int16_t)(((int16_t)buf[0]) << 8 | buf[1]));
	MPU6500_NewVal(&MPU6500_FIFO[1][0],(int16_t)(((int16_t)buf[2]) << 8 | buf[3]));
	MPU6500_NewVal(&MPU6500_FIFO[2][0],(int16_t)(((int16_t)buf[4]) << 8 | buf[5]));
	//温度数据
	MPU6500_NewVal(&MPU6500_FIFO[3][0],(int16_t)(((int16_t)buf[6]) << 8 | buf[7]));
	//角速度数据
	MPU6500_NewVal(&MPU6500_FIFO[4][0],(int16_t)(((int16_t)buf[8]) << 8 | buf[9]));
	MPU6500_NewVal(&MPU6500_FIFO[5][0],(int16_t)(((int16_t)buf[10]) << 8 | buf[11]));
	MPU6500_NewVal(&MPU6500_FIFO[6][0],(int16_t)(((int16_t)buf[12]) << 8 | buf[13]));
	//更新FIFO的指针
	Wr_Index = (Wr_Index + 1) % Buf_SIZE;
	//计算FIFO中的平均值
	gx =  MPU6500_GetAvg(&MPU6500_FIFO[4][0]);
	gy =  MPU6500_GetAvg(&MPU6500_FIFO[5][0]);
	gz =  MPU6500_GetAvg(&MPU6500_FIFO[6][0]);
	//角速度需要减去陀螺仪的飘移
	gyro[0] = gx - Gx_offset;	
	gyro[1] = gy - Gy_offset;
	gyro[2] = gz - Gz_offset;
			  
	ax = 	MPU6500_GetAvg(&MPU6500_FIFO[0][0]);
	ay = 	MPU6500_GetAvg(&MPU6500_FIFO[1][0]);
	az = 	MPU6500_GetAvg(&MPU6500_FIFO[2][0]);
	//返回滤波后的平均值			
	acc[0] = ax;
	acc[1] = ay;
	acc[2] = az;	
}


/**
 * @Description MPU6500 计算数据偏移量
 * 飞行器上电后、MPU测量的数据不是水平的、此时的数据就是偏移量
 */
void MPU6500_Init_Offset(void)
{
	unsigned int i;
	int16_t temp[3],temp2[3];
	int32_t	tempgx=0,tempgy=0,tempgz=0;
	int32_t tempax=0,tempay=0,tempaz=0;
	Gx_offset=0;
	Gy_offset=0;
	Gz_offset=0;
	//设置标志位
	Gyro_Off_started = 1;
	
	//先读取前面一些数据，丢弃不用
	for(i=0;i<50;i++){
  		delay_us(100);
		MPU6500_readGyro_Acc(temp,temp2);
	}
	//读取1000个数据、取平均值作为偏移量
 	for(i=0;i<1000;i++){
		delay_us(200);
		MPU6500_readGyro_Acc(temp,temp2);
		tempgx += temp[0];
		tempgy += temp[1];
		tempgz += temp[2];
		
		tempax += temp2[0];
		tempay += temp2[1];
		tempaz += temp2[2];
		
	}
	Gx_offset=tempgx/1000;
	Gy_offset=tempgy/1000;
	Gz_offset=tempgz/1000;
	//偏移量获取完成
	Gyro_Off_started = 0;
}
